#ifndef _MOTION_H_
#define _MOTION_H_

#include <cstring>
#include <iostream>
#include <thread>
#include <unistd.h>
#include <unitree/robot/go2/sport/sport_client.hpp>

#define PI 3.14159265358979323846 // 定义PI的值

class Motion {
private:
    unitree::robot::go2::SportClient spt;
    float dist_thr = 0.10; /* 10cm 内表示运动到位了 */
    float ang_thr = 0.2; /* 角度偏差大于 0.2 弧度, 换算成度就是 11.46度 */
    float low_lin_vel = 0.05; /* 角度偏差大的时候, 低速前进, 缩小转弯半径 */
    float max_lin_vel = 0.5; /* 最大线速度 */
    float max_ang_vel = 1.0; /* 最大角速度 */

public:
    Motion();
    ~Motion();
    void sport_position(float x, float y, float yaw);
    void lin_Vel_set(float vel);
    void move(float vx, float vy, float vyaw);
    void movestop();
    void msleep(int ms);
};

#endif
